#include"ros/ros.h"
#include"geometry_msgs/TransformStamped.h"
#include"tf2_ros/buffer.h"
#include"tf2_ros/transform_listener.h"
#include"tf2_geometry_msgs/tf2_geometry_msgs.h"
#include"geometry_msgs/Twist.h"

int main(int argc, char *argv[])
{
    //setlocale(LC_ALL,"");
    ros::init(argc,argv,"tes");
    ros::NodeHandle nh;
    tf2_ros::Buffer buffer;
    tf2_ros::TransformListener listen(buffer);
    ros::Rate rate(10);
    ros::Duration(3).sleep();
    ros::Publisher pub=nh.advertise<geometry_msgs::Twist>("/scout/cmd_vel",100);
    while(ros::ok())
    {
        geometry_msgs::TransformStamped stamp=buffer.lookupTransform("scout","turtle1",ros::Time(0));
        // ROS_INFO("偏移量为(%.2f,%.2f,%.2f)",
        //         stamp.transform.translation.x,
        //         stamp.transform.translation.y, 
        //         stamp.transform.translation.z
        //          );
        geometry_msgs::Twist twist;
        twist.linear.x=0.5*sqrt(pow(stamp.transform.translation.x,2)+pow(stamp.transform.translation.y,2));
        twist.angular.z=4*atan2(stamp.transform.translation.y,stamp.transform.translation.x);
        pub.publish(twist);
        rate.sleep();
    }

    ros::spinOnce();
    
    return 0;
}
